﻿// RoboKalleAbbanen versio 3.0

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using NKH.MindSqualls;
using NKH.MindSqualls.MotorControl;

namespace Microsoft.Samples.Kinect.SkeletonBasics
{
    class RoboKalle
    {
        public int kallerange = 1800;
        public int nopeusrajoitus = 50;
        public int kiihtyvyysrajoitus = 20;
        private char movemode = '1';
        private int viive = 10;
        private int sleeppi = 25;
        private NxtBrick kalle;
        private int ovkd;
        private int vvkd; // Edellisen tilan muuttujat
        private int sokd;
        private int svkd;
        private int sverotus;
        private int soerotus;
        private int sjerotus;
        private int jerotus;
        private int vjd;
        private int okd;  // Oikean käden sijaintimuuttuja
        private int vkd;  // Vasemman käden sijaintimuuttuja
        private int jd;   // Koiven sijaintimuuttuja
        private int sjd;
        private int verotus;
        private int oerotus;
        private int iii = 0;
        string logistringi;
        string vpoweri;
        string opoweri;
        string jpoweri;
        public bool kalleoff;
        public bool kallereunaan;
        Thread okasi_thread;
        Thread vkasi_thread;
        Thread jalat_thread;


        public RoboKalle()
        {

            kalleoff = false;
            kallereunaan = false;
            try
            {
                kalle = new NxtBrick(NxtCommLinkType.USB, 0);
            } catch {};
            try
            {
                kalle.MotorA = new NxtMotor(); // Vasen käsi
                kalle.MotorB = new NxtMotor(); // Oikea käsi
                kalle.MotorC = new NxtMotor(); // Jalat
            } catch {};
            try
            {
                MotorControlProxy.StartMotorControl(kalle.CommLink);
                System.Threading.Thread.Sleep(500); // Aloittaa MotorControl ohjelman robotilla ja odottaa hetken sen käynnistymistä
            }
            catch { };
            if (!kalle.IsConnected)
                kalleoff = true;

            okd = 0;
            vkd = 0;
            vvkd = 0;
            okasi_thread = new Thread(new ThreadStart(okasithread));
            okasi_thread.Start();
            vkasi_thread = new Thread(new ThreadStart(vkasithread));
            vkasi_thread.Start();
            jalat_thread = new Thread(new ThreadStart(jalatthread));
            jalat_thread.Start();

            okasi(180);
            vkasi(180);
            okd = 180;   // Kun kädet alhaalla, asetetaan oikean käden degree (okd) 180ksi
            vkd = 180;   // Sama vasemmalle
            jd = kallerange/2;
            vjd = kallerange/2;
            sjd = kallerange/2;
            jerotus = 0;
            //sjerotus = 0;
        }
        public void kuolekallekuole()
        {
            bootKalle();
            kalleoff = true;
            Thread.Sleep(200);            
            Thread.Sleep(sleeppi + 50);
            try
            {
                kalle.MotorA.Idle();
                kalle.MotorB.Idle();        	// Moottorit idlaamaan
                kalle.MotorC.Idle();
            } catch {};
            try
            {
                MotorControlProxy.StopMotorControl(kalle.CommLink); // Suljetaan ohjelma robotilla
            }
            catch { };
            okasi_thread.Abort();
            okasi_thread = null;
            vkasi_thread.Abort();
            vkasi_thread = null;
            jalat_thread.Abort();
            jalat_thread = null;
            kallereunaan = false;
        }


        public bool bootKalle()
        {
            okd = 0;
            vkd = 0;
            okasi(180);
            vkasi(180);
            jalat(0);
            kallereunaan = true;

            return true;
        }
        // VASEN KÄSI -->

        public bool vkasi(string pwr, string tacholm)
        {
            kalle.MotorA.Coast();
            if (!kalleoff)
            {
                bool done = false;
                do
                {
                    try
                    {
                        Console.Write("1");
                        if (MotorControlProxy.ISMOTORREADY(kalle.CommLink, MotorControlMotorPort.PortA))
                        {
                            Console.Write("2");
                            if (svkd < 20)
                            {
                                MotorControlProxy.CONTROLLED_MOTORCMD(kalle.CommLink, MotorControlMotorPort.PortA, "140", "000220", movemode);
                            }
                            else if (svkd > 160)
                            {
                                MotorControlProxy.CONTROLLED_MOTORCMD(kalle.CommLink, MotorControlMotorPort.PortA, "040", "000220", movemode);
                            }
                            else
                                MotorControlProxy.CONTROLLED_MOTORCMD(kalle.CommLink, MotorControlMotorPort.PortA, pwr, tacholm, movemode);
                            done = true;
                            vvkd = svkd;
                            verotus = 0;
                        }
                        else
                            Thread.Sleep(20);
                        Console.Write("3");
                    }
                    catch { };
                } while ((!done));// || (ticki < 10));
                Console.Write("!");
            }
            if (!kalleoff)
                kalle.MotorA.Poll();
            return true;
        }

        public void vkasithread()
        {

            bool loppu = false;
            while ((!loppu) || (!kalleoff))
            {
                if (kallereunaan)
                {
                    while (kallereunaan)
                    {
                    }
                    kalle.MotorA.Coast();
                }

                Thread.Sleep(sleeppi);
                sverotus = verotus;
                svkd = vkd;
                if (sverotus != 0)
                {
                    if (sverotus > 15) vpoweri = "100";   	// Powerin säätö, mitä isompi väli liikutaan, sitä nopeammin
                    else if (sverotus > 5) vpoweri = "070";
                    else if (sverotus > 2) vpoweri = "050";
                    else if (sverotus > 0) vpoweri = "020";
                    else if (sverotus > -2) vpoweri = "120";
                    else if (sverotus > -5) vpoweri = "150";
                    else if (sverotus > -15) vpoweri = "170";
                    else vpoweri = "200";

                    //if (verotus < 0)
                    //	vpoweri = "200";
                    //else
                    //	vpoweri = "100";

                    string termo;
                    int itseiserotus;
                    itseiserotus = Math.Abs(sverotus);    	//
                    termo = Convert.ToString(itseiserotus); //  Muunnetaan MotorControl-muotoon
                    termo = termo.PadLeft(6, '0');      	//
                    vkasi(vpoweri, termo);
                }
            };
        }


        public bool vkasi(int degr)
        {
            //vmoved = false;
            if ((((degr - vkd) < -viive)) || ((degr - vkd) > viive))
            {
                verotus = degr - vkd;	// Matka mikä liikutaan, verrataan vkdta, eli nyk. sijaintia pyydettyyn degreehen
                if (verotus < -180)
                    verotus = -180;
                if (verotus > 180)
                    verotus = 180;
                vkd += verotus;  // Säädetään raajan sijaintimuuttujaa erootuksella (joka on int-arvoinen, muuten sama kuin tacholm)
                if (vkd < 0)
                    vkd = 0;
                if (vkd > 180)
                    vkd = 180;
            }
            return true;
        }

        // OIKEA KÄSI -->


        public bool okasi(string pwr, string tacholm)
        {
            //try
            //{
            //Thread.Sleep(50);
            kalle.MotorB.Coast();
            //}
            //catch { };
            if (!kalleoff)
            {
                bool done = false;
                //  if (!vmoved)
                //{
                do
                {
                    try
                    {
                        Console.Write("1");
                        if (MotorControlProxy.ISMOTORREADY(kalle.CommLink, MotorControlMotorPort.PortB))
                        {
                            Console.Write("2");
                            if (sokd < 20)
                            {
                                MotorControlProxy.CONTROLLED_MOTORCMD(kalle.CommLink, MotorControlMotorPort.PortB, "140", "000220", movemode);
                            }
                            else if (sokd > 160)
                            {
                                MotorControlProxy.CONTROLLED_MOTORCMD(kalle.CommLink, MotorControlMotorPort.PortB, "040", "000220", movemode);
                            }
                            else
                                MotorControlProxy.CONTROLLED_MOTORCMD(kalle.CommLink, MotorControlMotorPort.PortB, pwr, tacholm, movemode);
                            done = true;
                            ovkd = sokd;
                            oerotus = 0;
                        }
                        else
                            Thread.Sleep(20);
                        Console.Write("3");
                    }
                    catch { };
                } while ((!done));// || (ticki < 10));
                Console.Write("!");
            }
            if (!kalleoff)
                kalle.MotorB.Poll();
            return true;
        }

        public void okasithread()
        {

            bool loppu = false;
            while ((!loppu) || (!kalleoff))
            {
                if (kallereunaan)
                {
                    while (kallereunaan)
                    {
                    }
                    kalle.MotorB.Coast();
                }

                //	Thread.Sleep(1);
                //	int oerotus = okd - ovkd;

                //if (oerotus > 50) opoweri = "100";   	// Powerin säätö, mitä isompi väli liikutaan, sitä nopeammin
                //else if (oerotus > 20) opoweri = "070";
                //else if (oerotus > 5) opoweri = "050";
                //else if (oerotus > -5) opoweri = "000";
                //else if (oerotus > -20) opoweri = "120";
                //else if (oerotus > -50) opoweri = "150";
                //else opoweri = "200";
                Thread.Sleep(sleeppi);
                soerotus = oerotus;
                sokd = okd;
                if (soerotus != 0)
                {
                    if (soerotus > 20) opoweri = "100";   	// Powerin säätö, mitä isompi väli liikutaan, sitä nopeammin
                    else if (soerotus > 10) opoweri = "070";
                    else if (soerotus > 5) opoweri = "050";
                    else if (soerotus > 0) opoweri = "020";
                    else if (soerotus > -5) opoweri = "120";
                    else if (soerotus > -10) opoweri = "150";
                    else if (soerotus > -20) opoweri = "170";
                    else opoweri = "200";
                    //if (oerotus < 0)
                    //	opoweri = "200";
                    //else
                    //	opoweri = "100";
                    string termo;
                    int itseiserotus;
                    itseiserotus = Math.Abs(soerotus);    	//
                    termo = Convert.ToString(itseiserotus); //  Muunnetaan MotorControl-muotoon
                    termo = termo.PadLeft(6, '0');      	//
                    //                	Console.Write(termo);
                    //              	Console.Write(opoweri);
                    //            	Console.Write(vmoved);
                    okasi(opoweri, termo);
                }
            };
        }


        public bool okasi(int degr)
        {
            if ((((degr - okd) < -viive)) || ((degr - okd) > viive))
            {
                oerotus = degr - okd;	// Matka mikä liikutaan, verrataan okdta, eli nyk. sijaintia pyydettyyn degreehen
                if (oerotus < -180)
                    oerotus = -180;
                if (oerotus > 180)
                    oerotus = 180;
                okd += oerotus;  // Säädetään raajan sijaintimuuttujaa erootuksella (joka on int-arvoinen, muuten sama kuin tacholm)
                if (okd < 0)
                    okd = 0;
                if (okd > 180)
                    okd = 180;
            }
            return true;
        }


        // JALAKSET -->


        public string jalat(string pwr, string tacholm)
        {
            kalle.MotorC.Coast();
            if (!kalleoff)
            {
                bool done = false;
                do
                {
                    try
                    {
                        if (MotorControlProxy.ISMOTORREADY(kalle.CommLink, MotorControlMotorPort.PortC))
                        {
                            if (tacholm != "000000")
                                MotorControlProxy.CONTROLLED_MOTORCMD(kalle.CommLink, MotorControlMotorPort.PortC, pwr, tacholm, movemode);
                            logistringi = " Power: " + pwr + " Tacholm: " + tacholm + " Sjd: " + sjd;
                            done = true;
                            vjd += jerotus;
                            jerotus = 0;
                            //sjerotus = 0;
                        }
                        else
                            Thread.Sleep(20);
                    }
                    catch { };
                } while ((!done));
                //try
                //{
                //    iii = 0;
                //    while (!MotorControlProxy.ISMOTORREADY(kalle.CommLink, MotorControlMotorPort.PortC))
                //    {
                //        Thread.Sleep(20);
                //        iii++;
                //    };
                //}
                //catch { };
            }
            if (!kalleoff)
                kalle.MotorC.Poll();
            return tacholm;
        }

        public string showme()
        {
            string stringer = kalle.MotorC.TachoCount.ToString();
            stringer += " ";
            stringer += iii.ToString();
            return stringer + logistringi;
        }

        private void kallereunalle()
        {
            bool done = false;
            if (!kalleoff)
            {
                kalle.MotorC.Coast();
                kalle.MotorA.Coast();
                kalle.MotorB.Coast();
                Thread.Sleep(sleeppi);
                while (!done)
                {
                    try
                    {
                        if (MotorControlProxy.ISMOTORREADY(kalle.CommLink, MotorControlMotorPort.PortC))
                        {
                            MotorControlProxy.CONTROLLED_MOTORCMD(kalle.CommLink, MotorControlMotorPort.PortC, "050", "001800", movemode);
                            done = true;
                            vjd = 0;
                            jd = 0;
                            sjd = 0;
                            jerotus = 0;
                            //sjerotus = 0;
                        }
                        else
                            Thread.Sleep(20);
                        if (MotorControlProxy.ISMOTORREADY(kalle.CommLink, MotorControlMotorPort.PortB))
                        {
                            MotorControlProxy.CONTROLLED_MOTORCMD(kalle.CommLink, MotorControlMotorPort.PortB, "050", "000220", movemode);
                            done = true;
                            vjd = 0;
                            jd = 0;
                            sjd = 0;
                            jerotus = 0;
                            //sjerotus = 0;
                        }
                        else
                            Thread.Sleep(20);
                        if (MotorControlProxy.ISMOTORREADY(kalle.CommLink, MotorControlMotorPort.PortA))
                        {
                            MotorControlProxy.CONTROLLED_MOTORCMD(kalle.CommLink, MotorControlMotorPort.PortA, "050", "000220", movemode);
                            done = true;
                            vjd = 0;
                            jd = 0;
                            sjd = 0;
                            jerotus = 0;
                            //sjerotus = 0;
                        }
                        else
                            Thread.Sleep(20);

                    }
                    catch { };
                    Thread.Sleep(20);
                }
            }

        }


        public void jalatthread()
        {
            kalle.MotorC.TriggerTachoCount = 1;
            kalle.MotorC.ResetMotorPosition(false);

            bool done = false;
            if (!kalleoff)
            {
                string termo;
                int itseiserotus;
                itseiserotus = Math.Abs(kallerange)/2;    	//
                termo = Convert.ToString(itseiserotus); //  Muunnetaan MotorControl-muotoon
                termo = termo.PadLeft(6, '0');      	//

                while (!done)
                {
                    try
                    {
                        if (MotorControlProxy.ISMOTORREADY(kalle.CommLink, MotorControlMotorPort.PortC))
                        {
                            MotorControlProxy.CONTROLLED_MOTORCMD(kalle.CommLink, MotorControlMotorPort.PortC, "200", termo, movemode);
                            done = true;
                        }
                        else
                            Thread.Sleep(20);
                    }
                    catch { };
                };
                Thread.Sleep(1000);
                kalle.MotorC.Coast();
            }
//            kallereunaan = true;
            bool loppu = false;
            while ((!loppu) || (!kalleoff))
            {
                if (kallereunaan)
                {
                    kallereunalle();
                    while (kallereunaan)
                    {
                    }
                    kalle.MotorC.Coast();
                }
                Thread.Sleep(sleeppi);
                sjd = jd;
                jerotus = (sjd - vjd);	// Matka mikä liikutaan, verrataan vjdta, eli nyk. sijaintia pyydettyyn degreehen
                if (jerotus < -nopeusrajoitus)
                    jerotus = -nopeusrajoitus;
                if (jerotus > nopeusrajoitus)
                    jerotus = nopeusrajoitus;
                if ((jerotus - sjerotus) > kiihtyvyysrajoitus)
                    jerotus = sjerotus + kiihtyvyysrajoitus;
                if ((jerotus - sjerotus) < -kiihtyvyysrajoitus)
                    jerotus = sjerotus - kiihtyvyysrajoitus;
                sjerotus = jerotus;

                if (jerotus != 0)
                {
                    //if (sjerotus > 15) jpoweri = "100";   	// Powerin säätö, mitä isompi väli liikutaan, sitä nopeammin
                    //else if (sjerotus > 5) jpoweri = "070";
                    //else if (sjerotus > 2) jpoweri = "050";
                    //else if (sjerotus > 0) jpoweri = "020";
                    //else if (sjerotus > -2) jpoweri = "120";
                    //else if (sjerotus > -5) jpoweri = "150";
                    //else if (sjerotus > -15) jpoweri = "170";
                    //else jpoweri = "200";

                    if (jerotus < 0)
                        jpoweri = "100";
                    else
                        jpoweri = "200";

                    string termo;
                    int itseiserotus;
                    itseiserotus = Math.Abs(jerotus);    	//
                    termo = Convert.ToString(itseiserotus); //  Muunnetaan MotorControl-muotoon
                    termo = termo.PadLeft(6, '0');      	//
                    if (!kalleoff)
                        jalat(jpoweri, termo);
                }
            };
        }


        public void jalat(double jx)
        {
            double jax = (0.7 + jx) * 1000.00;
            jax = (jax / 1400) * (double)kallerange;
            if (jax < 0.0)
                jax = 0.0;
            int degr = (int)jax;
            if ((((degr - jd) < (-viive * 2))) || ((degr - jd) > (viive * 2)))
            {
                jd = degr;
                if (jd < 0)
                    jd = 0;
                if (jd > kallerange)
                    jd = kallerange;
            }
        }
    }
}
